/* +---------------------------------------------------------------------------+
   |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
   |                                                                           |
   |                       http://www.mrpt.org/                                |
   |                                                                           |
   |   Copyright (C) 2005-2011  University of Malaga                           |
   |                                                                           |
   |    This software was written by the Machine Perception and Intelligent    |
   |      Robotics Lab, University of Malaga (Spain).                          |
   |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
   |                                                                           |
   |  This file is part of the MRPT project.                                   |
   |                                                                           |
   |     MRPT is free software: you can redistribute it and/or modify          |
   |     it under the terms of the GNU General Public License as published by  |
   |     the Free Software Foundation, either version 3 of the License, or     |
   |     (at your option) any later version.                                   |
   |                                                                           |
   |   MRPT is distributed in the hope that it will be useful,                 |
   |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
   |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
   |     GNU General Public License for more details.                          |
   |                                                                           |
   |     You should have received a copy of the GNU General Public License     |
   |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */


#include "splam.h"

using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::utils;
using namespace mrpt::slam;
using namespace mrpt::random;
using namespace mrpt::system;
using namespace std;

///////////////////////////////////////////////////////////////
//                      DECLARATION FOR SLAM                 //
///////////////////////////////////////////////////////////////

int main(int argc, char ** argv) {
    try {
        bool showHelp = argc > 1 && !os::_strcmp(argv[1], "--help");
        bool showVersion = argc > 1 && !os::_strcmp(argv[1], "--version");

        printf(" simul-landmarks - Part of the MRPT\n");
        printf(" MRPT C++ Library: %s - BUILD DATE %s\n", MRPT_getVersion().c_str(), MRPT_getCompilationDate().c_str());

        if (showVersion)
            return 0; // Program end
        //Set seed -- NO SEED
        randomGenerator.randomize();

        //Log directory
        deleteFilesInDirectory(OUT_DIR);
        createDirectory(OUT_DIR);

        // ---------------------------------------------
        // Create the point-beacons:
        // ---------------------------------------------
        printf("Creating landmark map...");
        mrpt::slam::CLandmarksMap landmarkMap;
        createLandmarkMap(landmarkMap);


        // ---------------------------------------------
        // Simulation parameters:
        // ---------------------------------------------
        Robot robot(0.5f, 0.1f);
        OdometrySensorModel odoModel = {odometryNoiseXY_std, odometryNoisePhi_std_deg};
        RangeSensorModel rangeModel = {minSensorDistance, maxSensorDistance, fieldOfView_deg, stdRange, stdYaw_deg};
        robot.setOdometryErrors(odoModel);
        robot.setSensorParameters(rangeModel);
        // ---------------------------------------------
        // Ground truth robot:
        // ---------------------------------------------
        CPose3D realPose;
        CMatrixDouble GT_path;
        CPose2D realPose2D(realPose);
        robot.setRealPose(realPose2D);
        // ---------------------------------------------
        // SLAM: variable declaration
        // --------------------------------------------- 
        CRangeBearingKFSLAM2D mapping;
        vector<TPose3D> meanPath; // The estimated path
        CPosePDFGaussian robotPose;
        std::vector<TPoint2D> LMs;
        std::map<unsigned int, CLandmark::TLandmarkID> LM_IDs;

        CActionCollectionPtr actions;
        CSensoryFramePtr observations;

        // ---------------------------------------------
        // Load the config options for mapping:
        // ---------------------------------------------        
        mapping.options.std_sensor_range = stdRange;
        mapping.options.quantiles_3D_representation = 3.0f;
        mapping.options.std_sensor_yaw = mrpt::math::DEG2RAD(stdYaw_deg);
        mapping.KF_options.method = (typename mrpt::bayes::TKFMethod)::kfEKFNaive;
        mapping.KF_options.dumpToConsole();
        mapping.options.dumpToConsole();


        // ---------------------------------------------
        // Display configuration:
        // ---------------------------------------------   
        CTicTac kftictac;

        //3D window
        Win3D myDisplay;

        //RandomWalk stuff
        Action lastAction(0,0);
        // ---------------------------------------------
        // Main Loop:
        // ---------------------------------------------  
        for (size_t i = 0; i < nSteps; i++) {
            //Stop key
            if (os::kbhit()) {
                char pushKey = os::getch();
                if (27 == pushKey)
                    break;
            }

            cout << "Generating step " << i << "...\n";
            //Declaration
            CMatrixDouble fullCov;
            CVectorDouble fullState;

            kftictac.Tic();

            //Simulate one step

            if (i >= N_STEPS_STOP_AT_THE_BEGINNING) {
                if (i % 15 == 0) {
                    lastV = randomGenerator.drawUniform(0, max_speed);
                    lastTheta = randomGenerator.drawUniform(-max_turn_rate, max_turn_rate);
                }
            } else {
                lastAction.turn_rate = 0;
                lastAction.velocity = 0;
            }
            robot.move(lastAction, SIZE_STEP_S);
            robot.sense(actions, observations, landmarkMap);
            robot.getRealPose(realPose);
            //Execute SLAM
            mapping.processActionObservation(actions, observations);
            //Retrieve result of SLAM
            mapping.getCurrentState(robotPose, LMs, LM_IDs, fullState, fullCov);
            cout << "Mean pose: " << endl << robotPose.mean << endl;
            cout << "# of landmarks in the map: " << LMs.size() << endl;

            const double tim_kf_iter = kftictac.Tac();

            // Keep the GT of the robot pose:
            GT_path.setSize(i + 1, 6);
            for (size_t k = 0; k < 6; k++)
                GT_path(i, k) = realPose[k];

            // Get the mean robot pose as 3D:
            const CPose3D robotPoseMean3D = CPose3D(robotPose.mean);
            // Build the path:
            meanPath.push_back(TPose3D(robotPoseMean3D));

            //Update the view
            myDisplay.repaint(mapping, meanPath, LMs, robotPose, robotPoseMean3D, GT_path, landmarkMap, tim_kf_iter, i);
        }

        actions.clear_unique();
        observations.clear_unique();

        // ---------------------------------------------
        // Outputs:
        // ---------------------------------------------  
        GT_path.saveToTextFile(
                               format("%s/ground_truth_robot_path.txt", OUT_DIR.c_str()),
                               MATRIX_FORMAT_FIXED,
                               true,
                               "% Columns are: x(m) y(m) z(m) yaw(rad) pitch(rad) roll(rad)\n");

        cout << "Data saved to directory: " << OUT_DIR << endl;
        //Output error on landmark estimate
        outputError(landmarkMap, LMs, LM_IDs);

        // ---------------------------------------------
        // End of the program:
        // ---------------------------------------------  
        cout << "********* KF-SLAM finished! **********" << endl;
        myDisplay.wait();

    } catch (std::exception &e) {
        std::cout << e.what();
    } catch (...) {
        std::cout << "Untyped exception!";
    }
}

void createLandmarkMap(mrpt::slam::CLandmarksMap &landmarkMap) {
    int uniqueIds = 1;
    for (size_t i = 0; i < nLandmarks; i++) {
        CLandmark LM;
        CPointPDFGaussian pt3D;

        // Random coordinates:
        pt3D.mean = CPoint3D(
                             randomGenerator.drawUniform(min_x, max_x),
                             randomGenerator.drawUniform(min_y, max_y),
                             0);

        // Add:
        LM.createOneFeature();
        LM.features[0]->type = featBeacon;
        LM.ID = uniqueIds++;
        LM.setPose(pt3D);

        landmarkMap.landmarks.push_back(LM);
    }
}

void outputError(const mrpt::slam::CLandmarksMap &landmarkMap, const std::vector<TPoint2D> &LMs, std::map<unsigned int, CLandmark::TLandmarkID> &LM_IDs) {

    if (LMs.size()) {
        // Each row has:
        //   [0] [1] [2]  [6]
        //    x   y   z    ID
        vector_double ERRS(0);
        for (size_t i = 0; i < LMs.size(); i++) {
            // Find the entry in the GT for this mapped LM:
            bool found = false;
            const CLandmark * true_LM = landmarkMap.landmarks.get(i);
            if (true_LM) {
                CPointPDFGaussian pose;
                CPoint3D gtPt;
                true_LM->getPose(pose);
                pose.getMean(gtPt);
                ERRS.push_back(gtPt.distanceTo(CPoint3D(TPoint3D(LMs[i])))); // All these conversions are to make it work with either CPoint3D & TPoint2D
            } else {
                cerr << "Ground truth entry not found for landmark ID:" << LM_IDs[i] << endl;
            }
        }

        printf("ERRORS VS. GROUND TRUTH:\n");
        printf("Mean Error: %f meters\n", math::mean(ERRS));
        printf("Minimum error: %f meters\n", math::minimum(ERRS));
        printf("Maximum error: %f meters\n", math::maximum(ERRS));
    }
}

void saveStep(const CPosePDFGaussian &robotPose, CMatrixDouble &fullCov, const CRangeBearingKFSLAM2D & mapping, const size_t &step) {
    // Save mean pose:
    if (!(step % SAVE_LOG_FREQUENCY)) {
        const vector_double p = robotPose.mean.getAsVectorVal();
        p.saveToTextFile(OUT_DIR + format("/robot_pose_%05u.txt", (unsigned int) step));
    }

    // Save full cov:
    if (!(step % SAVE_LOG_FREQUENCY)) {
        fullCov.saveToTextFile(OUT_DIR + format("/full_cov_%05u.txt", (unsigned int) step));
    }

    // Save map to file representations?
    if (SAVE_MAP_REPRESENTATIONS && !(step % SAVE_LOG_FREQUENCY)) {
        mapping.saveMapAndPath2DRepresentationAsMATLABFile(OUT_DIR + format("/slam_state_%05u.m", (unsigned int) step));
    }
}
